﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Aldebaran.Proxies;
using System.Net;
using System.Threading;
using System.Configuration;
using System.Collections;
using System.Collections.Concurrent;
using grounding.nao.enums;
using grounding.nao.sensors;

namespace grounding.nao
{
    public class Nao : ISensor
    {
        #region Motor Constraints ---------------------------------------------

        // Joint angle contraints in radians.
        private const float HEAD_PITCH_MAX = 0.5149f;
        private const float HEAD_PITCH_MIN = -0.6720f;
        private const float HEAD_YAW_MAX = 2.0857f;
        private const float HEAD_YAW_MIN = -2.0857f;
        private const float L_SHOULDER_PITCH_MAX = 2.0857f;
        private const float L_SHOULDER_PITCH_MIN = -2.0857f;
        private const float L_SHOULDER_ROLL_MAX = 1.3265f;
        private const float L_SHOULDER_ROLL_MIN = -0.3142f;
        private const float L_ELBOW_ROLL_MAX = -0.0349f;
        private const float L_ELBOW_ROLL_MIN = -1.5446f;
        private const float L_ELBOW_YAW_MAX = 2.0857f;
        private const float L_ELBOW_YAW_MIN = -2.0857f;
        private const float L_WRIST_YAW_MAX = 1.8238f;
        private const float L_WRIST_YAW_MIN = -1.8238f;
        private const float L_HIP_YAW_PITCH_MAX = 0.740810f;
        private const float L_HIP_YAW_PITCH_MIN = -1.145303f;
        private const float L_HIP_PITCH_MAX = 0.484090f;
        private const float L_HIP_PITCH_MIN = -1.773912f;
        private const float L_HIP_ROLL_MAX = 0.790477f;
        private const float L_HIP_ROLL_MIN = -0.379472f;
        private const float L_KNEE_PITCH_MAX = 2.112528f;
        private const float L_KNEE_PITCH_MIN = -0.092346f;
        private const float L_ANKLE_PITCH_MAX = 0.922747f;
        private const float L_ANKLE_PITCH_MIN = -1.189516f;
        private const float L_ANKLE_ROLL_MAX = 0.397880f;
        private const float L_ANKLE_ROLL_MIN = -0.769001f;
        private const float R_SHOULDER_PITCH_MAX = 2.0857f;
        private const float R_SHOULDER_PITCH_MIN = -2.0857f;
        private const float R_SHOULDER_ROLL_MAX = 0.3142f;
        private const float R_SHOULDER_ROLL_MIN = -1.3265f;
        private const float R_ELBOW_ROLL_MAX = 1.5446f;
        private const float R_ELBOW_ROLL_MIN = 0.0349f;
        private const float R_ELBOW_YAW_MAX = 2.0857f;
        private const float R_ELBOW_YAW_MIN = -2.0857f;
        private const float R_WRIST_YAW_MAX = 1.8238f;
        private const float R_WRIST_YAW_MIN = -1.8238f;
        private const float R_HIP_YAW_PITCH_MAX = 0.740810f;
        private const float R_HIP_YAW_PITCH_MIN = -1.145303f;
        private const float R_HIP_PITCH_MAX = 0.484090f;
        private const float R_HIP_PITCH_MIN = -1.773912f;
        private const float R_HIP_ROLL_MAX = 0.414754f;
        private const float R_HIP_ROLL_MIN = -0.738321f;
        private const float R_KNEE_PITCH_MAX = 2.120198f;
        private const float R_KNEE_PITCH_MIN = -0.103083f;
        private const float R_ANKLE_PITCH_MAX = 0.932056f;
        private const float R_ANKLE_PITCH_MIN = -1.186448f;
        private const float R_ANKLE_ROLL_MAX = 0.785875f;
        private const float R_ANKLE_ROLL_MIN = -0.388676f;

        #endregion ------------------------------------------------------------

        #region Constants -----------------------------------------------------

        private const string NAO_VOICE = "Kenny22Enhanced";
        private const float NAO_VOICE_VOLUME = 0.5f;

        private const string SONAR_SUBSCRIPTION = "sonarRecord";

        private float defaultMotorSpeed;
        private int sensorPollPeriod;

        private Mutex sensorPoll;

        #endregion ------------------------------------------------------------

        private MotionProxy motionProxy = null;
        private MemoryProxy memoryProxy = null;
        private RobotPoseProxy poseProxy = null;
        private TextToSpeechProxy ttsProxy = null;
        private SentinelProxy sentinelProxy = null;
        private SonarProxy sonarProxy = null;
        private FaceDetectionProxy faceDetectionProxy = null;

        // Maintain a dictionary of motor -> min/max angle.
        Dictionary<Motor, KeyValuePair<float, float>> angleLimits;

        // Sensors:
        private Timer sensorTimer;
        private Sonar sonar;
        private FootSensor footSensor;
        private TemperatureSensor tempSensor;
        private JointPositionSensor jointPositionSensor;
        private JointActuationSensor jointActuationSensor;
        private Accelerometer accelerometer;
        private BatterySensor batterySensor;
        private JointCurrentSensor jointCurrentSensor;
        private List<Sensor> activeSensors;
        private Dictionary<Sensor, Func<IEnumerable<double>, IEnumerable<double>>> transformingFuncs;

        /// <summary>
        /// The SensorFrameReceived event is called every once every poll cycle.  Use 
        /// this event to recieve data from the Nao's sensors.
        /// </summary>
        public event EventHandler<NaoSensorFrame> SensorFrameReceived;

        /// <summary>
        /// The IP address of the NAO robot to connect to
        /// </summary>
        public string naoIP { get; set; }

        /// <summary>
        /// The port that should be used to connect to the NAO robot
        /// </summary>
        public int naoPort { get; set; }

        /// <summary>
        /// Creates the NAO interface.  This uses Aldebaran's .Net NAOqi framework.
        /// </summary>
        /// <param name="ip">IP address of the NAO robot</param>
        /// <param name="port">Port to connect on</param>
        public Nao(string ip, int port, bool connect = true, Sensor[] active = null, Sensor[] averaged = null, Dictionary<Sensor, Func<IEnumerable<double>, IEnumerable<double>>> transformers = null)
        {
            naoIP = ip;
            naoPort = port;

            activeSensors = new List<Sensor>();
            if (active == null)
                activeSensors.AddRange(new Sensor[] { 
                    Sensor.Accelerometer, 
                    Sensor.BatterySensor, 
                    Sensor.FootSensor, 
                    Sensor.JointActuationSensor, 
                    Sensor.JointCurrentSensor, 
                    Sensor.JointPositionSensor, 
                    Sensor.Sonar, 
                    Sensor.TemperatureSensor });
            else
                activeSensors.AddRange(active);

            if (transformers == null)
                transformingFuncs = new Dictionary<Sensor, Func<IEnumerable<double>, IEnumerable<double>>>();
            else
                transformingFuncs = transformers;

            sensorPoll = new Mutex();

            // Build dictionary of angle limits.
            angleLimits = BuildAngleLimitDictionary();

            // Load config values from config file
            // NOTE: config file must exist in project with executable
            try
            {
                defaultMotorSpeed = float.Parse(ConfigurationManager.AppSettings["DefaultMotorSpeed"]);
                sensorPollPeriod = int.Parse(ConfigurationManager.AppSettings["SensorPollPeriod"]);
            }
            catch (Exception e)
            {
                Console.WriteLine("Couldn't load values from config file.  Failed with reason: " + e.Message);
                Console.WriteLine(e.StackTrace);
            }

            if (connect)
            {
                // Connect to robot
                if (!Connect())
                    throw new InvalidOperationException("Could not establish connection with NAO.");
            }
        }

        #region Connecting to the NAO robot -----------------------------------

        /// <summary>
        /// Establish connection to the NAO robot
        /// </summary>
        public bool Connect()
        {
            try
            {
                motionProxy = new MotionProxy(naoIP, naoPort);
                memoryProxy = new MemoryProxy(naoIP, naoPort);
                poseProxy = new RobotPoseProxy(naoIP, naoPort);
                ttsProxy = new TextToSpeechProxy(naoIP, naoPort);
                sentinelProxy = new SentinelProxy(naoIP, naoPort);
                sonarProxy = new SonarProxy(naoIP, naoPort);
                faceDetectionProxy = new FaceDetectionProxy(naoIP, naoPort);
            }
            catch (Exception e)
            {
                Console.WriteLine("Couldn't connect to NAO on " + e.Message);
                return false;
            }

            if (
                null == motionProxy ||
                null == memoryProxy ||
                null == poseProxy ||
                null == ttsProxy ||
                null == sonarProxy ||
                null == faceDetectionProxy ||
                !(
                motionProxy.ping() &&
                memoryProxy.ping() &&
                poseProxy.ping() &&
                ttsProxy.ping() &&
                sonarProxy.ping() &&
                faceDetectionProxy.ping()
                  )
                 )
            {
                motionProxy = null;
                memoryProxy = null;
                poseProxy = null;
                ttsProxy = null;
                sentinelProxy = null;
                sonarProxy = null;
                faceDetectionProxy = null;

                return false;
            }

            setupMotors();
            SetupSensors();

            return true;
        }

        /// <summary>
        /// Disconnects from NAO robot. Doesn't do anything if no connection has been
        /// made yet.
        /// </summary>
        public void Disconnect()
        {
            if (IsConnected)
            {
                // Reset NAO's state.
                //UnstiffAll();
                TeardownSensors();

                // Dispose proxies.
                motionProxy.Dispose();
                memoryProxy.Dispose();
                poseProxy.Dispose();
                ttsProxy.Dispose();
                sentinelProxy.Dispose();
                sonarProxy.Dispose();
                faceDetectionProxy.Dispose();

                // Remove reference to disposed proxies.
                motionProxy = null;
                memoryProxy = null;
                poseProxy = null;
                ttsProxy = null;
                sentinelProxy = null;
                sonarProxy = null;
                faceDetectionProxy = null;
            }
        }

        /// <summary>
        /// True if the a connection to a NAO robot has been made
        /// </summary>
        public bool IsConnected
        {
            get
            {
                return 
                    motionProxy != null &&
                    memoryProxy != null &&
                    poseProxy != null &&
                    ttsProxy != null &&
                    sentinelProxy != null &&
                    sonarProxy != null &&
                    faceDetectionProxy != null;
            }
        }

        #endregion ------------------------------------------------------------

        #region Motor Methods -------------------------------------------------

        private void setupMotors()
        {
        }

        public void ExtendArms()
        {
            if (IsConnected)
            {
                ArrayList names = new ArrayList();
                ArrayList angles = new ArrayList();

                names.Add("LShoulderPitch");
                angles.Add(0.0f);
                names.Add("LShoulderRoll");
                angles.Add(0.0f);
                names.Add("LElbowYaw");
                angles.Add(-0.5f);
                names.Add("LElbowRoll");
                angles.Add(0.0f);
                names.Add("LWristYaw");
                angles.Add(-1.8f);
                names.Add("RShoulderPitch");
                angles.Add(0.0f);
                names.Add("RShoulderRoll");
                angles.Add(0.0f);
                names.Add("RElbowYaw");
                angles.Add(0.5f);
                names.Add("RElbowRoll");
                angles.Add(0.0f);
                names.Add("RWristYaw");
                angles.Add(1.8f);

                motionProxy.setAngles(names, angles, defaultMotorSpeed);
            }
        }

        public void ExtendLeftArm()
        {
            if (IsConnected)
            {
                ArrayList names = new ArrayList();
                ArrayList angles = new ArrayList();

                names.Add("LShoulderPitch");
                angles.Add(0.0f);
                names.Add("LShoulderRoll");
                angles.Add(0.0f);
                names.Add("LElbowYaw");
                angles.Add(-0.5f);
                names.Add("LElbowRoll");
                angles.Add(0.0f);
                names.Add("LWristYaw");
                angles.Add(-1.8f);

                motionProxy.setAngles(names, angles, defaultMotorSpeed);
            }
        }

        public void ExtendRightArm()
        {
            if (IsConnected)
            {
                ArrayList names = new ArrayList();
                ArrayList angles = new ArrayList();

                names.Add("RShoulderPitch");
                angles.Add(0.0f);
                names.Add("RShoulderRoll");
                angles.Add(0.0f);
                names.Add("RElbowYaw");
                angles.Add(0.5f);
                names.Add("RElbowRoll");
                angles.Add(0.0f);
                names.Add("RWristYaw");
                angles.Add(1.8f);

                motionProxy.setAngles(names, angles, defaultMotorSpeed);
            }
        }

        /// <summary>
        /// Grabs whatever is in the NAO's hands.
        /// </summary>
        public void Grab()
        {
            if (IsConnected)
            {
                motionProxy.closeHand("LHand");
                motionProxy.closeHand("RHand");
            }
        }

        /// <summary>
        /// Closess NAO's left hand
        /// </summary>
        public void GrabLeft()
        {
            if (IsConnected)
                motionProxy.closeHand("LHand");
        }

        /// <summary>
        /// Closess NAO's right hand
        /// </summary>
        public void GrabRight()
        {
            if (IsConnected)
                motionProxy.closeHand("RHand");
        }

        /// <summary>
        /// Lets go of whatever is in the NAO's hands.
        /// </summary>
        public void UnGrab()
        {
            if (IsConnected)
            {
                motionProxy.openHand("LHand");
                motionProxy.openHand("RHand");
            }
        }

        /// <summary>
        /// Lets go of whatever is in the NAO's left hand.
        /// </summary>
        public void UnGrabLeft()
        {
            if (IsConnected)
                motionProxy.openHand("LHand");
        }

        /// <summary>
        /// Lets go of whatever is in the NAO's left hand.
        /// </summary>
        public void UnGrabRight()
        {
            if (IsConnected)
                motionProxy.openHand("RHand");
        }

        /// <summary>
        /// Sets a NAO joint to a specified angle.
        /// </summary>
        /// <param name="m">Motor to update</param>
        /// <param name="angle">Angle to set motor to</param>
        public void SetMotorAngle(Motor m, float angle)
        {
            if (IsConnected)
            {
                float minAngle = angleLimits[m].Key;
                float maxAngle = angleLimits[m].Value;

                float naoAngle = Math.Max(Math.Min(DegreesToRadians(angle), maxAngle), minAngle);

                motionProxy.setAngles(m.ToString(), naoAngle, defaultMotorSpeed);
            }
        }

        /// <summary>
        /// Updates a set of motors to a set of angles in one instruction.
        /// This should be faster than sending each motor command individually.
        /// </summary>
        /// <param name="d">a dictionary of Motor (enum) -> angle (float) mappings.</param>
        public void SetMotorAnglesBatch(Dictionary<Motor, float> d)
        {
            if (IsConnected)
            {
                ArrayList names = new ArrayList();
                ArrayList angles = new ArrayList();

                foreach (Motor m in d.Keys)
                {
                    float minAngle = angleLimits[m].Key;
                    float maxAngle = angleLimits[m].Value;

                    float naoAngle = Math.Max(Math.Min(DegreesToRadians(d[m]), maxAngle), minAngle);

                    names.Add(m.ToString());
                    angles.Add(naoAngle);
                }

                motionProxy.setAngles(names, angles, defaultMotorSpeed);
            }
        }

        /// <summary>
        /// Walk the specified direction the specified amount of meters.
        /// </summary>
        /// <param name="meters">meters to walk</param>
        /// <param name="d">direction to walk</param>
        public void Walk(float meters, Direction d)
        {
            if (IsConnected)
            {
                switch (d)
                {
                    case Direction.Forward:
                        motionProxy.walkTo(meters, 0.0f, 0.0f);

                        break;
                    case Direction.Backward:
                        motionProxy.walkTo(-meters, 0.0f, 0.0f);

                        break;
                    case Direction.Left:
                        motionProxy.walkTo(0.0f, meters, 0.0f);

                        break;
                    case Direction.Right:
                        motionProxy.walkTo(0.0f, -meters, 0.0f);

                        break;
                }
            }
        }

        /// <summary>
        /// Performs a previously recorded gesture on the NAO.
        /// 
        /// NOTE: This is assumed to be a type
        /// SkeletonDataType.ArmAngles gesture.
        /// </summary>
        /// <param name="name">the name of the gesture</param>
        public void PerformGesture(string name)
        {
            SkeletonDataParser dataParser =
                new SkeletonDataParser(name, SkeletonDataType.ArmAngles);

            // Load the skeleton data from disk.
            dataParser.LoadSkeletonData();

            // Just grab the first sample of motion.
            Dictionary<Motor, List<KeyValuePair<DateTime, float>>> angles =
                dataParser.fetchArmAngles()[0];

            // Pose NAO into first frame from data.
            Dictionary<Motor, float> motorCmd = new Dictionary<Motor, float>();
            motorCmd[Motor.LShoulderPitch] = angles[Motor.LShoulderPitch][0].Value;
            motorCmd[Motor.LShoulderRoll] = angles[Motor.LShoulderRoll][0].Value;
            motorCmd[Motor.LElbowRoll] = angles[Motor.LElbowRoll][0].Value;
            motorCmd[Motor.LElbowYaw] = angles[Motor.LElbowYaw][0].Value;
            motorCmd[Motor.RShoulderPitch] = angles[Motor.RShoulderPitch][0].Value;
            motorCmd[Motor.RShoulderRoll] = angles[Motor.RShoulderRoll][0].Value;
            motorCmd[Motor.RElbowRoll] = angles[Motor.RElbowRoll][0].Value;
            motorCmd[Motor.RElbowYaw] = angles[Motor.RElbowYaw][0].Value;

            SetMotorAnglesBatch(motorCmd);

            // Wait a short time for the NAO to reach this pose.
            // TODO: Instead, wait as long as it takes for the NAO to stop moving.
            System.Threading.Thread.Sleep(1000);

            // Continue with the gesture;
            for (int step = 1; step < angles[Motor.LShoulderPitch].Count; step++)
            {
                motorCmd[Motor.LShoulderPitch] = angles[Motor.LShoulderPitch][step].Value;
                motorCmd[Motor.LShoulderRoll] = angles[Motor.LShoulderRoll][step].Value;
                motorCmd[Motor.LElbowRoll] = angles[Motor.LElbowRoll][step].Value;
                motorCmd[Motor.LElbowYaw] = angles[Motor.LElbowYaw][step].Value;
                motorCmd[Motor.RShoulderPitch] = angles[Motor.RShoulderPitch][step].Value;
                motorCmd[Motor.RShoulderRoll] = angles[Motor.RShoulderRoll][step].Value;
                motorCmd[Motor.RElbowRoll] = angles[Motor.RElbowRoll][step].Value;
                motorCmd[Motor.RElbowYaw] = angles[Motor.RElbowYaw][step].Value;

                SetMotorAnglesBatch(motorCmd);

                // Wait the duration between steps before proceeding.
                System.Threading.Thread.Sleep(
                    (int)(angles[Motor.LShoulderPitch][step].Key.ToFileTime() -
                    angles[Motor.LShoulderPitch][step - 1].Key.ToFileTime()) / 10000);
            }
        }

        /// <summary>
        /// Sets stiffness of a motor to 1.0f
        /// </summary>
        /// <param name="motor">Motor to stiff</param>
        public void StiffMotor(Motor motor)
        {
            if (IsConnected)
                motionProxy.setStiffnesses(motor.ToString(), 1.0f);
            else
                throw new InvalidOperationException("Can't stiff motors when not connected to a NAO");
        }

        /// <summary>
        /// Sets stiffness of a motor to 0.0f
        /// </summary>
        /// <param name="motor">Motor to unstiff</param>
        public void UnstiffMotor(Motor motor)
        {
            if (IsConnected)
                motionProxy.setStiffnesses(motor.ToString(), 0.0f);
            else
                throw new InvalidOperationException("Can't unstiff motors when not connected to a NAO");
        }

        /// <summary>
        /// Sets stiffness of all of the NAO's motors to 1.0f
        /// </summary>
        public void StiffAll()
        {
            if (IsConnected)
                motionProxy.setStiffnesses("Body", 1.0f);
            else
                throw new InvalidOperationException("Can't stiff motors when not connected to a NAO");
        }

        /// <summary>
        /// Sets stiffness of all the NAO's motors to 0.0f
        /// </summary>
        public void UnstiffAll()
        {
            if (IsConnected)
                motionProxy.setStiffnesses("Body", 0.0f);
            else
                throw new InvalidOperationException("Can't unstiff motors when not connected to a NAO");
        }

        /// <summary>
        /// Construct a dictionary of motor -> min/max angle mappings.
        /// </summary>
        /// <returns>a new dictionary of motor -> min/max angle mappings</returns>
        private Dictionary<Motor, KeyValuePair<float, float>> BuildAngleLimitDictionary()
        {
            Dictionary<Motor, KeyValuePair<float, float>> d = new Dictionary<Motor, KeyValuePair<float, float>>();

            d.Add(Motor.HeadPitch, new KeyValuePair<float, float>(HEAD_PITCH_MIN, HEAD_PITCH_MAX));
            d.Add(Motor.HeadYaw, new KeyValuePair<float, float>(HEAD_YAW_MIN, HEAD_YAW_MAX));
            d.Add(Motor.LShoulderPitch, new KeyValuePair<float, float>(L_SHOULDER_PITCH_MIN, L_SHOULDER_PITCH_MAX));
            d.Add(Motor.RShoulderPitch, new KeyValuePair<float, float>(R_SHOULDER_PITCH_MIN, R_SHOULDER_PITCH_MAX));
            d.Add(Motor.LShoulderRoll, new KeyValuePair<float, float>(L_SHOULDER_ROLL_MIN, L_SHOULDER_ROLL_MAX));
            d.Add(Motor.RShoulderRoll, new KeyValuePair<float, float>(R_SHOULDER_ROLL_MIN, R_SHOULDER_ROLL_MAX));
            d.Add(Motor.LElbowRoll, new KeyValuePair<float, float>(L_ELBOW_ROLL_MIN, L_ELBOW_ROLL_MAX));
            d.Add(Motor.RElbowRoll, new KeyValuePair<float, float>(R_ELBOW_ROLL_MIN, R_ELBOW_ROLL_MAX));
            d.Add(Motor.LElbowYaw, new KeyValuePair<float, float>(L_ELBOW_YAW_MIN, L_ELBOW_YAW_MAX));
            d.Add(Motor.RElbowYaw, new KeyValuePair<float, float>(R_ELBOW_YAW_MIN, R_ELBOW_YAW_MAX));
            d.Add(Motor.LWristYaw, new KeyValuePair<float, float>(L_WRIST_YAW_MIN, L_WRIST_YAW_MAX));
            d.Add(Motor.RWristYaw, new KeyValuePair<float, float>(R_WRIST_YAW_MIN, R_WRIST_YAW_MAX));
            d.Add(Motor.LHipYawPitch, new KeyValuePair<float, float>(L_HIP_YAW_PITCH_MIN, L_HIP_YAW_PITCH_MAX));
            d.Add(Motor.RHipYawPitch, new KeyValuePair<float, float>(R_HIP_YAW_PITCH_MIN, R_HIP_YAW_PITCH_MAX));
            d.Add(Motor.LHipPitch, new KeyValuePair<float, float>(L_HIP_PITCH_MIN, L_HIP_PITCH_MAX));
            d.Add(Motor.RHipPitch, new KeyValuePair<float, float>(R_HIP_PITCH_MIN, R_HIP_PITCH_MAX));
            d.Add(Motor.LHipRoll, new KeyValuePair<float, float>(L_HIP_ROLL_MIN, L_HIP_ROLL_MAX));
            d.Add(Motor.RHipRoll, new KeyValuePair<float, float>(R_HIP_ROLL_MIN, R_HIP_ROLL_MAX));
            d.Add(Motor.LKneePitch, new KeyValuePair<float, float>(L_KNEE_PITCH_MIN, L_KNEE_PITCH_MAX));
            d.Add(Motor.RKneePitch, new KeyValuePair<float, float>(R_KNEE_PITCH_MIN, R_KNEE_PITCH_MAX));
            d.Add(Motor.LAnklePitch, new KeyValuePair<float, float>(L_ANKLE_PITCH_MIN, L_ANKLE_PITCH_MAX));
            d.Add(Motor.RAnklePitch, new KeyValuePair<float, float>(R_ANKLE_PITCH_MIN, R_ANKLE_PITCH_MAX));
            d.Add(Motor.LAnkleRoll, new KeyValuePair<float, float>(L_ANKLE_ROLL_MIN, L_ANKLE_ROLL_MAX));
            d.Add(Motor.RAnkleRoll, new KeyValuePair<float, float>(R_ANKLE_ROLL_MIN, R_ANKLE_ROLL_MAX));

            return d;
        }

        #endregion

        #region Utility Methods -----------------------------------------------

        /// <summary>
        /// Says the given string on the NAO.
        /// </summary>
        /// <param name="speech">string of text to synthesize</param>
        public void Say(string speech)
        {
            ttsProxy.setVoice(NAO_VOICE);
            ttsProxy.setVolume(NAO_VOICE_VOLUME);

            if (IsConnected)
                ttsProxy.say(speech);
        }

        /// <summary>
        /// Stops current and pending TTS tasks
        /// </summary>
        public void StopTalking()
        {
            ttsProxy.stopAll();
        }

        /// <summary>
        /// Gets some bit of data from the memory proxy.
        /// </summary>
        /// <param name="item">key for the memory desired</param>
        /// <returns>memory associated with key; null if not connected
        /// or invalid key</returns>
        public object GetMemory(string item)
        {
            try
            {
                return memoryProxy.getData(item);
            }
            catch
            {
                return null;
            }
        }

        /// <summary>
        /// Check the current battery level.
        /// </summary>
        /// <param name="sayPercentage">wether or not the NAO should speak its battery level</param>
        /// <returns>the approximate percentage of battery remaining (0.0 - 1.0)</returns>
        public float BatteryReport(bool sayPercentage)
        {
            int currentLevel = sentinelProxy.getBatteryLevel();

            if (sayPercentage)
            {
                switch (currentLevel)
                {
                    case 0:
                        Say("My battery is near empty");
                        break;

                    case 1:
                        Say("My battery is at approximately twenty percent capacity.");
                        break;

                    case 2:
                        Say("My battery is at approximately forty percent capacity.");
                        break;

                    case 3:
                        Say("My battery is at approximately sixty percent capacity.");
                        break;

                    case 4:
                        Say("My battery is at approximately eighty percent capacity.");
                        break;

                    case 5:
                        Say("My battery is full.");
                        break;
                }
            }

            return currentLevel / 5.0f;
        }

        /// <summary>
        /// Converts degrees to radians.
        /// </summary>
        /// <param name="angle">angle in degrees</param>
        /// <returns>radian value</returns>
        private float DegreesToRadians(double angle)
        {
            return (float)(Math.PI * angle / 180.0);
        }

        /// <summary>
        /// Converts radians to degrees.
        /// </summary>
        /// <param name="angle">angle in radians</param>
        /// <returns>degree value</returns>
        private float RadiansToDegrees(double angle)
        {
            return (float)(angle * 180.0 / Math.PI);
        }

        #endregion ------------------------------------------------------------

        #region Sensor Methods ------------------------------------------------

        /// <summary>
        /// Prepare sensors for use.
        /// </summary>
        private void SetupSensors()
        {
            if (IsConnected)
            {
                // Turn on sonar.
                sonarProxy.subscribe(SONAR_SUBSCRIPTION);

                // Create Sensor Objects
                if (activeSensors.Contains(Sensor.Sonar))
                    sonar = new Sonar(sonarProxy, memoryProxy, "sonar");
                if (activeSensors.Contains(Sensor.FootSensor))
                    footSensor = new FootSensor(memoryProxy, "foot");
                if (activeSensors.Contains(Sensor.TemperatureSensor))
                    tempSensor = new TemperatureSensor(memoryProxy, "temperature");
                if (activeSensors.Contains(Sensor.JointPositionSensor))
                    jointPositionSensor = new JointPositionSensor(memoryProxy, "joint_position");
                if (activeSensors.Contains(Sensor.JointActuationSensor))
                    jointActuationSensor = new JointActuationSensor(memoryProxy, "joint_actuation");
                if (activeSensors.Contains(Sensor.Accelerometer))
                    accelerometer = new Accelerometer(memoryProxy, "acceleration");
                if (activeSensors.Contains(Sensor.BatterySensor))
                    batterySensor = new BatterySensor(sentinelProxy, "battery");
                if (activeSensors.Contains(Sensor.JointCurrentSensor))
                    jointCurrentSensor = new JointCurrentSensor(memoryProxy, "joint_current");

                sensorTimer = new Timer(new TimerCallback(pollSensors), null, 0, sensorPollPeriod);
            }
        }

        private void pollSensors(object obj)
        {
            //Console.WriteLine("Polling Sensors...");
            if (SensorFrameReceived != null)
                SensorFrameReceived(this, (NaoSensorFrame)GetSensorFrame());
        }

        /// <summary>
        /// Disable the sensors and reset the NAO's state back to normal.
        /// </summary>
        public void TeardownSensors()
        {
            if (IsConnected)
            {
                // Unsubscribe from all sonar messages; this should turn off sonar.
                ArrayList subscriptions = (ArrayList)sonarProxy.getSubscribersInfo();
                foreach (ArrayList subscription in subscriptions)
                    sonarProxy.unsubscribe((string)subscription[0]);
            }
        }

        /// <summary>
        /// Checks if the NAO's current pose is close enough to
        /// one of the known existing poses:
        ///  * Stand
        ///  * Sit
        ///  * Crouch
        ///  * Knee
        ///  * Frog
        ///  * Back
        ///  * Belly
        ///  * Left
        ///  * Right
        ///  * HeadBack
        /// </summary>
        /// <returns>Best determined Pose, or Pose.Unknown if the pose cannot
        /// be determined.</returns>
        public Pose getCurrentPose()
        {
            if (IsConnected)
            {
                Pose p;
                if (Enum.TryParse((string)((ArrayList)poseProxy.getActualPoseAndTime())[0], out p))
                    return p;
            }

            return Pose.Unknown;
        }

        /// <summary>
        /// Checks if the NAO's current pose is close enough to
        /// one of the known existing poses:
        ///  * Stand
        ///  * Sit
        ///  * Crouch
        ///  * Knee
        ///  * Frog
        ///  * Back
        ///  * Belly
        ///  * Left
        ///  * Right
        ///  * HeadBack
        /// </summary>
        /// <param name="sayPose">wether or not the NAO should say it's current pose.</param>
        /// <returns>Best determined Pose, or Pose.Unknown if the pose cannot
        /// be determined.</returns>
        public Pose getCurrentPose(bool sayPose)
        {
            Pose p = getCurrentPose();
            
            Say("Hello, I am now, and I am currently in a " + p.ToString() + " position.");

            return p;
        }

        public List<ISensorFrame> GetSensorFrames(int repetitions, int msBetweenCollections = 200)
        {
            List<ISensorFrame> frames = new List<ISensorFrame>();
            for (int i = 0; i < repetitions; i++)
            {
                frames.Add(GetSensorFrame());
                Thread.Sleep(msBetweenCollections);
            }
            return frames;
        }

        #endregion ------------------------------------------------------------

        #region ISensor Members

        /// <summary>
        /// Returns the string "NAO" as the name for the nao robot sensors
        /// </summary>
        public string Name { get { return "NAO"; } }

        /// <summary>
        /// Returns the string "NAO" as the type for the nao robot sensors
        /// </summary>
        public string Type{get { return "NAO"; }}

        public ISensorFrame GetSensorFrame()
        {
            sensorPoll.WaitOne();

            ArrayList keys = new ArrayList();

            //
            // Specify keys for the memory proxy.
            //

            // Sonar.
            keys.Add("Device/SubDeviceList/US/Left/Sensor/Value");
            keys.Add("Device/SubDeviceList/US/Right/Sensor/Value");

            // Foot Sensor.
            keys.Add("Device/SubDeviceList/LFoot/FSR/FrontLeft/Sensor/Value");
            keys.Add("Device/SubDeviceList/LFoot/FSR/FrontRight/Sensor/Value");
            keys.Add("Device/SubDeviceList/LFoot/FSR/RearLeft/Sensor/Value");
            keys.Add("Device/SubDeviceList/LFoot/FSR/RearRight/Sensor/Value");
            keys.Add("Device/SubDeviceList/RFoot/FSR/FrontLeft/Sensor/Value");
            keys.Add("Device/SubDeviceList/RFoot/FSR/FrontRight/Sensor/Value");
            keys.Add("Device/SubDeviceList/RFoot/FSR/RearLeft/Sensor/Value");
            keys.Add("Device/SubDeviceList/RFoot/FSR/RearRight/Sensor/Value");

            // Temperature Sensor.
            keys.Add("Device/SubDeviceList/Battery/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/HeadPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnklePitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnkleRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowYaw/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHand/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipYawPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LKneePitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/LWristYaw/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnklePitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnkleRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowYaw/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHand/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RKneePitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderPitch/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderRoll/Temperature/Sensor/Value");
            keys.Add("Device/SubDeviceList/RWristYaw/Temperature/Sensor/Value");

            // Joint Position Sensor.
            keys.Add("Device/SubDeviceList/HeadPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/HeadYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnklePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnkleRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHand/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipYawPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LKneePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/LWristYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnklePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnkleRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowYaw/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHand/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RKneePitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderPitch/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderRoll/Position/Sensor/Value");
            keys.Add("Device/SubDeviceList/RWristYaw/Position/Sensor/Value");

            // Joint Actuation Sensor.
            keys.Add("Device/SubDeviceList/HeadPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/HeadYaw/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LAnklePitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LAnkleRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LElbowRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LElbowYaw/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LHand/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LHipPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LHipRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LHipYawPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LKneePitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LShoulderPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LShoulderRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/LWristYaw/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RAnklePitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RAnkleRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RElbowRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RElbowYaw/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RHand/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RHipPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RHipRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RKneePitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RShoulderPitch/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RShoulderRoll/Position/Actuator/Value");
            keys.Add("Device/SubDeviceList/RWristYaw/Position/Actuator/Value");

            // Accelerometer.
            keys.Add("Device/SubDeviceList/InertialSensor/AccX/Sensor/Value");
            keys.Add("Device/SubDeviceList/InertialSensor/AccY/Sensor/Value");
            keys.Add("Device/SubDeviceList/InertialSensor/AccZ/Sensor/Value");

            // Joint Current Sensor.
            keys.Add("Device/SubDeviceList/HeadPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/HeadYaw/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnklePitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LAnkleRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LElbowYaw/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHand/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LHipYawPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LKneePitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LShoulderRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/LWristYaw/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnklePitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RAnkleRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RElbowYaw/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHand/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RHipRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RKneePitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderPitch/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RShoulderRoll/ElectricCurrent/Sensor/Value");
            keys.Add("Device/SubDeviceList/RWristYaw/ElectricCurrent/Sensor/Value");

            // Fetch the data.
            ArrayList rawValues = (ArrayList)memoryProxy.getListData(keys);

            //
            // Convert to arrays of doubles.
            //

            int c;

            // Sonar.
            double[] sonarValues = new double[1];
            sonarValues[0] = ((float)rawValues[0] + (float)rawValues[1]) / 2;

            // Foot Pressure.
            c = 0;
            double[] footValues = new double[8];
            for (int i = 2; i < 10; i++)
                footValues[c++] = (float)rawValues[i];

            // Temperatures.
            c = 0;
            double[] temperatureValues = new double[25];
            for (int i = 10; i < 35; i++)
                temperatureValues[c++] = (float)rawValues[i];

            // Joint Positions.
            c = 0;
            double[] jointPositionValues = new double[25];
            for (int i = 35; i < 60; i++)
                jointPositionValues[c++] = (float)rawValues[i];

            // Joint Actuation.
            c = 0;
            double[] jointActuationValues = new double[25];
            for (int i = 60; i < 85; i++)
                jointActuationValues[c++] = (float)rawValues[i];

            // Accelerometer.
            c = 0;
            double[] accelerometerValues = new double[3];
            for (int i = 85; i < 88; i++)
                accelerometerValues[c++] = (float)rawValues[i];

            // Joint Current.
            c = 0;
            double[] jointCurrentValues = new double[25];
            for (int i = 88; i < 113; i++)
                jointCurrentValues[c++] = (float)rawValues[i];

            //
            // Create sensor frames.
            //

            List<SensorFrame> frames = new List<SensorFrame>();

            if (activeSensors.Contains(Sensor.Sonar))
                frames.Add(new SonarSensorFrame(sonar, (transformingFuncs.ContainsKey(Sensor.Sonar)) ? transformingFuncs[Sensor.Sonar](sonarValues).ToArray() : sonarValues));
            if (activeSensors.Contains(Sensor.FootSensor))
                frames.Add(new FootSensorFrame(footSensor, (transformingFuncs.ContainsKey(Sensor.FootSensor)) ? transformingFuncs[Sensor.FootSensor](footValues).ToArray() : footValues));
            if (activeSensors.Contains(Sensor.TemperatureSensor))
                frames.Add(new TemperatureSensorFrame(tempSensor, (transformingFuncs.ContainsKey(Sensor.TemperatureSensor)) ? transformingFuncs[Sensor.TemperatureSensor](temperatureValues).ToArray() : temperatureValues));
            if (activeSensors.Contains(Sensor.JointPositionSensor))
                frames.Add(new JointPositionSensorFrame(jointPositionSensor, (transformingFuncs.ContainsKey(Sensor.JointPositionSensor)) ? transformingFuncs[Sensor.JointPositionSensor](jointPositionValues).ToArray() : jointPositionValues));
            if (activeSensors.Contains(Sensor.JointActuationSensor))
                frames.Add(new JointActuationSensorFrame(jointActuationSensor, (transformingFuncs.ContainsKey(Sensor.JointActuationSensor)) ? transformingFuncs[Sensor.JointActuationSensor](jointActuationValues).ToArray() : jointActuationValues));
            if (activeSensors.Contains(Sensor.Accelerometer))
                frames.Add(new AccelerometerFrame(accelerometer, (transformingFuncs.ContainsKey(Sensor.Accelerometer)) ? transformingFuncs[Sensor.Accelerometer](accelerometerValues).ToArray() : accelerometerValues));
            if (activeSensors.Contains(Sensor.BatterySensor))
                frames.Add((SensorFrame)batterySensor.GetSensorFrame(transformingFuncs.ContainsKey(Sensor.BatterySensor) ? transformingFuncs[Sensor.BatterySensor] : (x => x)));
            if (activeSensors.Contains(Sensor.JointCurrentSensor))
                frames.Add(new JointCurrentSensorFrame(jointCurrentSensor, (transformingFuncs.ContainsKey(Sensor.JointCurrentSensor)) ? transformingFuncs[Sensor.JointCurrentSensor](jointCurrentValues).ToArray() : jointCurrentValues));

            NaoSensorFrame sensorData = new NaoSensorFrame(this, frames);

            sensorPoll.ReleaseMutex();

            // Build composite NaoSensorFrame
            return sensorData;
        }

        // Don't want to deal with this right now.
        public ISensorFrame GetSensorFrame(Func<IEnumerable<double>, IEnumerable<double>> transformer)
        {
            throw new NotImplementedException();
        }

        #endregion
    }
}
